%% initialization
clear all
% x = [0:21 21 20]; 
% y = 1+[0 0 0.5 2.8 2.8 2 1.5 1.4 2 2.8 2.8 2.8 2.8 2.8 2.8 2.8 2.8 2.8 2.8 2.8 2.8 2.5 1 0];
% x_c=[0 -1 -1 -1 0]+1;
% y_c=[-1 -1 0 1 1];
% REAL Coke bottle
x=[0 1 1.8 1.8 2 2.2 2.2 3:29 28.5 28.2 27.8 27.5];
y=[1.3 1.3 1.3 1.8 1.8 1.8 1.5 1.6 2.1 2.5 2.9 3.2 3.6 3.9 4 4 4 4 4 4 4 4 4 4 3.9 3.8 3.7 3.6 3.8 3.9 4 4 3.6 3 2.3 1.8 1.5 0];
x_c=[1.4 0 0 0 1.4];
y_c=[-1.5 -1.5 0 1.5 1.5];
close all;

liquid_volume = 100; % units?
liquid_density = 5; % 
bottle_mass = 73; %Grams
cap_mass = 1; %Grams
%cap_mass_center = [0;0];
%% bottle model depiction
[bottle_mass_center, bottle_shape, cap_mass_center] = bottle_model(x, y, x_c, y_c);
%% mode parameters
% - mode_name                 - 'mesh' or 'monte_carlo'
% - monte_carlo_points_number - number of points in monte carlo integration
% - vertices                  - coordinates of vertices in triangulation
%                               (we make triangulation in tipping_angle)
% - triangles                 - 
% - rotation_angle            - rotation angle (you should set this
%                               parameter before drawing plots in mesh mode)
mode_parameters.mode_name = 'mesh';
mode_parameters.monte_carlo_points_number = 500;
edge_length = 3;%min_bottle_poly_distance(bottle_shape);
[mode_parameters.vertices mode_parameters.triangles] = triangulation(bottle_shape, edge_length);
mode_parameters.rotation_angle = 0;
%% tipping_angle
tic;
[angle mass_center] = tipping_angle(liquid_volume, liquid_density, bottle_shape, bottle_mass,...
                                      bottle_mass_center, cap_mass, cap_mass_center, mode_parameters);
toc
%angle
angle * 180 / pi
mass_center
%%start picture
figure;
mode_parameters.rotation_angle = 0;

%fill(bottle_shape(1,:), bottle_shape(2,:), [0.5 0.5 0.9]);
hold on
plot(bottle_mass_center(1), bottle_mass_center(2), 'xg', 'LineWidth', 2, 'MarkerSize', 10);

plot(cap_mass_center(1), cap_mass_center(2), 'xk', 'LineWidth', 2, 'MarkerSize', 10);

start_water_mass_center = liquid_mass_center(liquid_volume, bottle_shape, mode_parameters);
plot(start_water_mass_center(1), start_water_mass_center(2), 'xb', 'LineWidth', 2, 'MarkerSize', 10);

system_mass = liquid_volume * liquid_density + bottle_mass + cap_mass;
system_mass_center = (liquid_volume * liquid_density * start_water_mass_center +...
                     bottle_mass * bottle_mass_center + cap_mass * cap_mass_center) / system_mass;
plot(system_mass_center(1), system_mass_center(2), 'xr', 'LineWidth', 2, 'MarkerSize', 10);

plot(bottle_shape(1,:), bottle_shape(2,:), 'k-');

start_level = liquid_level(liquid_volume, bottle_shape, mode_parameters);
plot([min(bottle_shape(1,:)) max(bottle_shape(1,:))], [start_level start_level], 'b-');


legend('bottle m_c', 'cap m_c', 'liquid m_c', 'system m_c');
axis equal
%%end picture
figure;
mode_parameters.rotation_angle = angle;
rotated_bottle_shape = rotation_matrix(angle) * bottle_shape;
rotated_bottle_mass_center = rotation_matrix(angle) * bottle_mass_center;
rotated_cap_mass_center = rotation_matrix(angle) * cap_mass_center;
water_mass_center = liquid_mass_center(liquid_volume, rotated_bottle_shape, mode_parameters);
end_level = liquid_level(liquid_volume, rotated_bottle_shape, mode_parameters);


plot(rotated_bottle_mass_center(1), rotated_bottle_mass_center(2), 'xg', 'LineWidth', 2, 'MarkerSize', 10);
hold on
plot(rotated_cap_mass_center(1), rotated_cap_mass_center(2), 'xk', 'LineWidth', 2, 'MarkerSize', 10);
plot(water_mass_center(1), water_mass_center(2), 'xb', 'LineWidth', 2, 'MarkerSize', 10);
plot(mass_center(1), mass_center(2), 'xr', 'LineWidth', 2, 'MarkerSize', 10);
plot([0 0], [min(rotated_bottle_shape(2,:)) max(rotated_bottle_shape(2,:))], 'g--');
plot(rotated_bottle_shape(1,:), rotated_bottle_shape(2,:), 'k-');
plot([min(rotated_bottle_shape(1,:)) max(rotated_bottle_shape(1,:))], [end_level end_level], 'b-');
legend('bottle m_c', 'cap m_c', 'liquid m_c', 'system m_c');
axis equal